//
// Created by daybeha on 2022/3/8.
//

//subscriiber ID20(0x14)  data package

#include <ros/ros.h>
#include <iostream>
#include <unistd.h>
#include <serial_imu/NAV100_0x1C.h>
#include "ch_serial.h"

#include <sensor_msgs/Imu.h>

#define RAD_TO_DEG   180/M_PI

void imu_0x1C_filted_callback(const sensor_msgs::ImuConstPtr& imu);
void imu_0x1C_callback(const serial_imu::NAV100_0x1CConstPtr& imu);
void imu_sensor_callback(const sensor_msgs::ImuConstPtr& imu);



int main(int argc,char **argv)
{
    ros::init(argc,argv,"sub_0x1C");

    ros::NodeHandle n;

    bool if_filted;
    n.param<bool>("/if_filted",if_filted, false );

    ros::Subscriber imu_0x14_sub, imu_sensor_sub;

    if (if_filted){
        imu_0x14_sub = n.subscribe("/imu/data", 20, imu_0x1C_filted_callback);
    } else{
        imu_0x14_sub = n.subscribe("/nav100_0x1C_package", 20, imu_0x1C_callback);
    }

//    imu_sensor_sub = n.subscribe("/IMU_data", 20, imu_sensor_callback);

    ros::spin();
}


uint32_t time_start;
bool if_first = true;
void imu_0x1C_filted_callback(const sensor_msgs::ImuConstPtr& imu)
{
    printf("\033c");

    printf("     Devie ID: %s\n", imu->header.frame_id.c_str());

    if(if_first){
        time_start = imu->header.stamp.sec;
        if_first = false;
    }

    uint32_t time =  imu->header.stamp.sec - time_start;
//    uint64_t time = imu.header.stamp.toNSec();
    printf("    Run times: %d days  %d:%d:%d:%d\n", time / 86400000, time / 3600000 % 24, time / 60000 % 60, time / 1000 % 60, time % 1000);
//    printf("    Run times: %f\n", imu->header.stamp.toSec());
//
//    printf("   Frame Rate:  %4dHz\r\n", imu_0x1C_msg.imu_data.frame_rate);
//
    printf("   Acc(m/s^2):%8.3f %8.3f %8.3f\r\n", imu->linear_acceleration.x, imu->linear_acceleration.y, imu->linear_acceleration.z);
//    printf("   Eul(R P Y):%8.2f %8.2f %8.2f\r\n", imu_0x14_msg.imu_data.eul_r*RAD_TO_DEG, imu_0x14_msg.imu_data.eul_p*RAD_TO_DEG, imu_0x14_msg.imu_data.eul_y*RAD_TO_DEG);
    printf("   Gyr(rad/s):%8.2f %8.2f %8.2f\r\n", imu->angular_velocity.x,imu->angular_velocity.y, imu->angular_velocity.z);
    printf("Quat(W X Y Z):%8.3f %8.3f %8.3f %8.3f\r\n", imu->orientation.w, imu->orientation.x, imu->orientation.y, imu->orientation.z);
}


void imu_0x1C_callback(const serial_imu::NAV100_0x1CConstPtr& imu)
{
    printf("\033c");

    printf("     Devie ID:%6d\n", imu->imu_data.id);
//
    printf("    Run times: %d days  %d:%d:%d:%d\n", imu->imu_data.time / 86400000, imu->imu_data.time / 3600000 % 24, imu->imu_data.time / 60000 % 60, imu->imu_data.time / 1000 % 60, imu->imu_data.time % 1000);
//
    printf("   Frame Rate:  %4dHz\r\n", imu->imu_data.frame_rate);
//
//    printf("   Gravity(g):  %8.3f\r\n", imu_0x14_msg.imu_data.gravity);

//    printf("          GPS:%8.3f %8.3f %8.3f\r\n",imu_0x14_msg.imu_data.Latitude, imu_0x14_msg.imu_data.Longitude, imu_0x14_msg.imu_data.Height);
//    printf("   GPS dev(m):%8.3f %8.3f %8.3f\r\n",imu_0x14_msg.imu_data.Latitude_dev, imu_0x14_msg.imu_data.Longitude_dev, imu_0x14_msg.imu_data.Height_dev);
//
//    printf("Velocity(m/s):%8.3f %8.3f %8.3f\r\n",imu_0x14_msg.imu_data.v_north, imu_0x14_msg.imu_data.v_east, imu_0x14_msg.imu_data.v_down);

    printf("   Acc(m/s^2):%8.3f %8.3f %8.3f\r\n", imu->imu_data.acc_x, imu->imu_data.acc_y, imu->imu_data.acc_z);
//    printf("   Eul(R P Y):%8.2f %8.2f %8.2f\r\n", imu_0x14_msg.imu_data.eul_r*RAD_TO_DEG, imu_0x14_msg.imu_data.eul_p*RAD_TO_DEG, imu_0x14_msg.imu_data.eul_y*RAD_TO_DEG);
    printf("   Gyr(rad/s):%8.2f %8.2f %8.2f\r\n", imu->imu_data.gyr_x, imu->imu_data.gyr_y, imu->imu_data.gyr_z);
    printf("Quat(W X Y Z):%8.3f %8.3f %8.3f %8.3f\r\n", imu->imu_data.quat_w, imu->imu_data.quat_x, imu->imu_data.quat_y, imu->imu_data.quat_z);
}


void imu_sensor_callback(const sensor_msgs::ImuConstPtr& imu)
{
    printf("\033c");

    printf("     Devie ID: %s\n", imu->header.frame_id.c_str());

    if(if_first){
        time_start = imu->header.stamp.sec;
        if_first = false;
    }

    uint32_t time =  imu->header.stamp.sec - time_start;
    printf("    Run times: %d days  %d:%d:%d:%d\n", time / 86400000, time / 3600000 % 24, time / 60000 % 60, time / 1000 % 60, time % 1000);

//
    printf("   Acc(m/s^2):%8.3f %8.3f %8.3f\r\n", imu->linear_acceleration.x, imu->linear_acceleration.y, imu->linear_acceleration.z);
//    printf("   Eul(R P Y):%8.2f %8.2f %8.2f\r\n", imu_0x14_msg.imu_data.eul_r*RAD_TO_DEG, imu_0x14_msg.imu_data.eul_p*RAD_TO_DEG, imu_0x14_msg.imu_data.eul_y*RAD_TO_DEG);
    printf("   Gyr(rad/s):%8.2f %8.2f %8.2f\r\n", imu->angular_velocity.x,imu->angular_velocity.y, imu->angular_velocity.z);
    printf("Quat(W X Y Z):%8.3f %8.3f %8.3f %8.3f\r\n", imu->orientation.w, imu->orientation.x, imu->orientation.y, imu->orientation.z);
}
